/**
  ******************************************************************************
  * @file           : power_ctrl_drv.cpp
  * @author         : Tessia
  * @brief          : None
  * @attention      : None
  * @date           : 2024/3/20
  ******************************************************************************
  */



#include "power_ctrl_drv.h"

extern receive_judge_t judge_rece_mesg;
extern uint8_t is_use_capacitor;
extern SuperCapState super_cap_state;
extern Super_Cap_t superCap;

ChassisPowerDataStruct Power;

void power_param_init(){
    Power.R = 0.004f; //0.002f;
    Power.kk = 7.0f; //7.0f;
    Power.kp_Kincrease = 0.002f;
    Power.kp_Kincrease2 = 0.0025f;
    Power.K_Power_Buffer = 0.042f;
    Power.upHill_k[0] = 0.5f;
    Power.upHill_k[1] = 0.5f;
    Power.upHill_k[2] = 1.5f;
    Power.upHill_k[3] = 1.5f;
}

/*!
 * @brief 获取当前缓冲能量
 * @return 缓冲能量大小
 */
float GetPowerBuffer(){
    //使用超级电容时，缓冲能量限制在某个值以上
    if (is_use_capacitor == 1){
        uint16_t buffer = judge_rece_mesg.power_heat_data.chassis_power_buffer < 35 ? 35 : judge_rece_mesg.power_heat_data.chassis_power_buffer;
        if(buffer>250) buffer = 60;//避免数据接收错误，限制缓冲能量大小,
        return buffer;
    }
    return judge_rece_mesg.power_heat_data.chassis_power_buffer;
}

float powerIncrement=0;
float GetPowerLimit(){
    float chassis_power_limit = judge_rece_mesg.robot_status_data.chassis_power_limit;
    /* 错误检测 */

    if(super_cap_state == SUPERC_ERROR || super_cap_state == SUPERC_DEBUG) //超级电容丢失时使用裁判系统数据
        return judge_rece_mesg.robot_status_data.chassis_power_limit;

    //使用超级电容，功率上限提高
    if(is_use_capacitor == 1){
        //拟合一条随电容电压而变化增加功率的曲线
        powerIncrement = -0.0003f * powf(superCap.scFeedback.V_SC,4) + 0.4024f * powf(superCap.scFeedback.V_SC,2)-104.46f;
        if(superCap.scFeedback.V_SC<=19.0f)
        {
            powerIncrement=0;
        }

        if(powerIncrement >= 30.f)
        {
            powerIncrement = 30.f;
        }
        else if(powerIncrement <= 0)
        {
            powerIncrement = 0;
        }
        chassis_power_limit= powerIncrement + (float)judge_rece_mesg.robot_status_data.chassis_power_limit;
    }

    //当电容空时，避免超功率，不提升功率大小
    if(super_cap_state==EMPTY)
    {
        chassis_power_limit = (float)judge_rece_mesg.robot_status_data.chassis_power_limit;
    }

    return chassis_power_limit;
}


/*!
 * @brief 获取当前功率
 * @return  当前功率
 */
float GetPowerNow(){

    //超级电容测得功率更准，优先使用超级电容测得功率。当超级电容失联时则使用裁判系统数据
    if(super_cap_state == SUPERC_ERROR || super_cap_state == SUPERC_DEBUG)//超级电容丢失
        return judge_rece_mesg.power_heat_data.chassis_power;
    if(superCap.power_fb.p_wheel == 0 && superCap.power_fb.p_sc == 0 &&
       (superCap.scFeedback.V_SC > 27.0f || superCap.scFeedback.V_SC <= 0.f)) //电容数据出问题
        return judge_rece_mesg.power_heat_data.chassis_power;

    return SC_get_power_wheel();

}

void ChassisWheelControl(float *current_speed_array, float *vel_pid_out_array) {
    //************功率输入********************//
    Power.powerall_ax = 0;
    Power.powerall_bx = 0;
    Power.Buffer = GetPowerBuffer();
    Power.Judge_Limit = GetPowerLimit();
    Power.NowPower = GetPowerNow();

    /*
     * 当前底盘功率管控方案
     * 由于底盘功率存在限制（限制功率大小为Power.NowPower)，发送电流不能直接按PID输出发送，而需要乘以系数k（即Power.k)使得电机功率满足限制功率要求
     * 为计算当前限制功率下需要的电流大小，用以下公式计算
     * P_limit= (k * I)^2 * R + (k * I) * v_speed * K_k
     * 其中，P_limit即为当前限制功率(Power.LimitedPower_Now)， I为速度环PID计算结果，k为电流乘的系数(即Power.k)，K_k,R为一参数
     * 方案理解：底盘功率可视为热损耗与输出机械功率两部分，(k * I)^2 * R即为电阻热损耗， (k * I) * v_speed * K_k为输出机械功率
     *         机械功率为T*omega=C_t*phi*I*omega正比于电流I*v_speed(omega为转速rpm,具体转矩T公式参考电机学)，K_k即为固定参数
     *         同样的，R电阻也为固定参数，但R与K_k均未知，只有自己调参（理想情况下R，K_k均接近实际参数，但参数不一定调的准，故采用以下方案）：
     *         将K_k利用负反馈设置为动态参数，当当前功率小于限制功率时，减小K_k值以增大k
     * */

    //***************解算一个内部使用的功率限制********//

    float K_LimitedPower_Max;
    if(Power.Judge_Limit < 60)
        K_LimitedPower_Max = 1.2f;
    else
        K_LimitedPower_Max = 1.3f;

    //限制功率参数，该参数正比于缓冲能量大小与当前限制功率大小
    float K_LimitedPower = K_LimitedPower_Max - 4.0f * expf(-Power.K_Power_Buffer * Power.Buffer);         //动态的P参数。
    if (Power.NowPower > Power.Judge_Limit && Power.Buffer < 15.0f) K_LimitedPower = 0.2f; //0.2f;       //马上就要超功率了
    if (K_LimitedPower < 0.1) K_LimitedPower = 0.1f;        //最低自我限制为0.1
    if (Power.Buffer < 10.0f) K_LimitedPower = 0.1f;

    Power.LimitedPower_Now = Power.Judge_Limit * K_LimitedPower;

/////*************速度解算和功率限制*******************///////////

//这部分里原来有一个陀螺模式下的移动限幅度 同理，限制平移速度的东西并不需要写在功率环里面啊。
//同时看到处心积虑的去控制平移和旋转的比例，应当先搞清楚你做事的目的是什么 如果在移动过程中保证陀螺速度
//那么如上，去限速的函数里面去写
//如果是功率环导致陀螺移动不走直线，说明是轮速没有达到设定速度，同样回到限速环里面去改
//如果是陀螺速度慢了，那么直接去陀螺解算函数里面改啊
//请不要在功率环里面搅限速的屎


    for (int i = 0; i < 4; i++) {

        //根据速度环PID计算需要发给电调的电流数据大小，但由于存在超功率风险，需要计算这个PID输出需要乘的系数即Power.k
        Power.powerall_ax += Power.R * vel_pid_out_array[i] * vel_pid_out_array[i];
        Power.powerall_bx += Power.kk * current_speed_array[i] * vel_pid_out_array[i];
    }
    if (Power.powerall_ax <= 0)
        Power.powerall_ax = 0.000001f;

    Power.k = (-Power.powerall_bx + sqrtf(Power.powerall_bx * Power.powerall_bx + 4.f * Power.powerall_ax * (Power.LimitedPower_Now) / 80.0f)) /(2.f * Power.powerall_ax); //求根公式
    if (Power.k > 1){
        //如果功率没有超限，k不用起作用
        Power.k = 1;
    }
    else
    {
        if (Power.powerall_ax != 0 && Power.powerall_bx != 0){
            Power.kk = Power.kk + Power.kp_Kincrease * (Power.NowPower - Power.LimitedPower_Now);//负反馈调节kk值
        }
        if (Power.kk < 4)       Power.kk = 4;
        else if (Power.kk > 22) Power.kk = 22;
    }
    if (Power.Buffer < 10.0f) Power.k = 0.0f; //防止超功率死掉
}
